#include "stm32f10x.h"                  // Device header
#include "JDY68A.h"
#include "Serial.h"
#include "PID.h"
#include "OLED.h"

extern PID_t LeftPID[];
extern PID_t RightPID[];

extern uint8_t status1;	// 启动标志位

#define adds	0.01f	// PID调试每次加减值

#define MOTORx	0		// 0内环，1外环

u8 Fore, Back, Left, Right; // 定义方向控制标志位（前进、后退、左转、右转）

typedef enum {
    CMD_STOP = 0x49,		// 停止指令
    CMD_BACK = 0x4B,		// 后退指令
    CMD_RIGHT = 0x4A,		// 右转指令
    CMD_FORWARD = 0x47,		// 前进指令
    CMD_LEFT = 0x48			// 左转指令
} BluetoothCommand;

void JDY68A_Init(void)
{
	Serial_Init();
	
}

void PIDDebug(void)
{
	if(Serial_GetRxFlag() == 1)
	{
		int8_t data = Serial_GetRxData();

		switch(data)
		{
			case 'q':
				LeftPID[MOTORx].Kp = LeftPID[MOTORx].Kp + adds;
				break;
			case 'w':
				LeftPID[MOTORx].Kp = LeftPID[MOTORx].Kp - adds;
				break;
			case 'e':
				LeftPID[MOTORx].Ki = LeftPID[MOTORx].Ki + adds;
				break;
			case 'r':
				LeftPID[MOTORx].Ki = LeftPID[MOTORx].Ki - adds;
				break;
			case 't':
				LeftPID[MOTORx].Kd = LeftPID[MOTORx].Kd + adds;
				break;
			case 'y':
				LeftPID[MOTORx].Kd = LeftPID[MOTORx].Kd - adds;
				break;
			case 'a':
				RightPID[MOTORx].Kp = RightPID[MOTORx].Kp + adds;
				break;
			case 's':
				RightPID[MOTORx].Kp = RightPID[MOTORx].Kp - adds;
				break;
			case 'd':
				RightPID[MOTORx].Ki = RightPID[MOTORx].Ki + adds;
				break;
			case 'f':
				RightPID[MOTORx].Ki = RightPID[MOTORx].Ki - adds;
				break;
			case 'g':
				RightPID[MOTORx].Kd = RightPID[MOTORx].Kd + adds;
				break;
			case 'h':
				RightPID[MOTORx].Kd = RightPID[MOTORx].Kd - adds;
				break;
			case 'z':
				status1 = !status1;
				break;
			default:
				break;
		}
		
		Serial_Printf("%f, %f, %f\r\n%f, %f, %f",LeftPID[MOTORx].Kp, LeftPID[MOTORx].Ki, LeftPID[MOTORx].Kd, 
													RightPID[MOTORx].Kp, RightPID[MOTORx].Ki, RightPID[MOTORx].Kd);
	}
	
	
	
//	OLED_Printf(64, 0, OLED_6X8, "Kp:%+05.0f", LeftPID[0].Kp);
//	OLED_Printf(64, 8, OLED_6X8, "Ki:%+05.0f", LeftPID[0].Ki);
//	OLED_Printf(64, 16, OLED_6X8, "Kd:%+05.0f", LeftPID[0].Kd);
//	
//	OLED_Printf(64, 24, OLED_6X8, "Kp:%+05.0f", LeftPID[0].Kp);
//	OLED_Printf(64, 32, OLED_6X8, "Ki:%+05.0f", LeftPID[0].Ki);
//	OLED_Printf(64, 40, OLED_6X8, "Kd:%+05.0f", LeftPID[0].Kd);
	
//	OLED_Printf(0, 16, OLED_6X8, "sta:%05d", status1);
	
//	Serial_Printf("%f, %f, %f",LeftPID[0].Target, LeftPID[0].Actual);
}

//void USART3_IRQHandler(void)
//{
//	int Bluetooth_data;  // 存储接收到的蓝牙数据
//    
//    // 检查是否发生接收中断
//    if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
//    {
//        Bluetooth_data = USART_ReceiveData(USART3);
//        
//        // 方向标志设置
//        switch((BluetoothCommand)Bluetooth_data) {
//            case CMD_STOP:    // 停止指令
//                Fore = 0; Back = 0; Left = 0; Right = 0;
//                break;
//                
//            case CMD_FORWARD: // 前进指令
//                Fore = 1; Back = 0; Left = 0; Right = 0;
//                break;
//                
//            case CMD_BACK:    // 后退指令
//                Fore = 0; Back = 1; Left = 0; Right = 0;
//                break;
//                
//            case CMD_RIGHT:   // 右转指令
//                Fore = 0; Back = 0; Left = 0; Right = 1;
//                break;
//                
//            case CMD_LEFT:    // 左转指令
//                Fore = 0; Back = 0; Left = 1; Right = 0;
//                break;
//                
//            default:          // 未知指令，视为停止
//                Fore = 0; Back = 0; Left = 0; Right = 0;
//                break;
//        }
//        
//        // 清除中断标志（确保可靠处理）
//        USART_ClearITPendingBit(USART3, USART_IT_RXNE);
//    }
//}
